﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;

namespace NinjaToolbox.PhysicsSystem
{
    public class FallingHeadDetector
    {
        Bounds mBoxcastCacheBounds;

        Transform[] mIgnoreTransformsArray;

        Transform mTransform;
        Vector3 mFixCenterOffsetPoint;
        float mGradient;
        float mRadius;
        float mMinSpeed;
        float mMaxSpeed;
        float mSpeedAreaTopExtern;
        float mSpeedAreaBottomExtern;
        float mError = 0.001f;
        bool mDebug_CurrentIsRuning;
        Vector3? mDebug_ContactPoint;

        public bool IsInit { get { return mTransform != null; } }
        public Vector3 FixCenterOffsetPoint { get { return mFixCenterOffsetPoint; } set { mFixCenterOffsetPoint = value; } }
        public float Gradient { get { return mGradient; } set { mGradient = value; } }
        public float Radius { get { return mRadius; } set { mRadius = value; } }
        public float MinSpeed { get { return mMinSpeed; } set { mMinSpeed = value; } }
        public float MaxSpeed { get { return mMaxSpeed; } set { mMaxSpeed = value; } }
        public float SpeedAreaTopExtern { get { return mSpeedAreaTopExtern; } set { mSpeedAreaTopExtern = value; } }
        public float SpeedAreaBottomExtern { get { return mSpeedAreaBottomExtern; } set { mSpeedAreaBottomExtern = value; } }
        public float Error { get { return mError; } set { mError = value; } }


        public void Init(Transform transform, params Transform[] ignoreTransformsArray)
        {
            mTransform = transform;
            mIgnoreTransformsArray = ignoreTransformsArray;

            var center = mFixCenterOffsetPoint - Vector3.up * mGradient * 0.5f;
            var size = new Vector3(mRadius * 2f, mGradient, mRadius * 2f);
            mBoxcastCacheBounds = new Bounds(center, size);
        }

        public void Update()
        {
            mDebug_ContactPoint = null;
            mDebug_CurrentIsRuning = false;

            PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(ForeachRegistedPhysicsObject);
        }

        public void DrawGizmos()
        {
            var cacheColor = Gizmos.color;

            var center = mFixCenterOffsetPoint - Vector3.up * mGradient * 0.5f;
            var diameter = mRadius * 2f;
            var size = new Vector3(diameter, mGradient, diameter);
            mBoxcastCacheBounds = new Bounds(center, size);

            Gizmos.color = mDebug_CurrentIsRuning ? Color.red : Color.blue;

            var topCenter = mTransform.position + mFixCenterOffsetPoint;
            var yAxis = -Physics.gravity.normalized;
            var bottomCenter = topCenter - yAxis * mGradient;
            var forward = Vector3.forward;
            var lastPoint = (Vector3?)null;

            for (int delta = 30, i = -delta; i < 360; i += delta)
            {
                var quat = Quaternion.AngleAxis(i, Physics.gravity.normalized);
                var target = topCenter + quat * forward * mRadius;

                var dir = (target - topCenter).normalized;
                var bottomPoint = bottomCenter + Vector3.ProjectOnPlane(dir, yAxis) * mRadius;

                Gizmos.DrawLine(bottomCenter, bottomPoint);
                Gizmos.DrawLine(bottomCenter, bottomPoint);
                Gizmos.DrawLine(bottomPoint, topCenter);

                if (lastPoint != null)
                    Gizmos.DrawLine(lastPoint.Value, bottomPoint);

                lastPoint = bottomPoint;
            }

            if (mDebug_ContactPoint != null)
            {
                Gizmos.color = Color.white;
                Gizmos.DrawWireSphere(mDebug_ContactPoint.Value, 0.1f);
            }

            Gizmos.color = Color.blue;
            Gizmos.DrawWireCube(mTransform.position + mBoxcastCacheBounds.center, mBoxcastCacheBounds.size);

            Gizmos.color = Color.yellow;
            Gizmos.DrawLine(topCenter + yAxis * mSpeedAreaTopExtern, bottomCenter - yAxis * mSpeedAreaBottomExtern);

            Gizmos.color = cacheColor;
        }

        void ForeachRegistedPhysicsObject(PhysicsObject physicsObject)
        {
            if (!mBoxcastCacheBounds.Contains(mTransform.worldToLocalMatrix.MultiplyPoint3x4(physicsObject.transform.position))) return;

            if (System.Array.Find(mIgnoreTransformsArray, m => m == physicsObject.transform) != null) return;

            mDebug_ContactPoint = physicsObject.transform.position;

            var isFix = ConeFix(physicsObject.transform);

            if (isFix)
                mDebug_CurrentIsRuning = true;
        }

        //          * -(center point)
        //         * *
        //        *   *
        //       *     *
        bool ConeFix(Transform target)
        {
            var topCenterPoint = mTransform.position + mFixCenterOffsetPoint;

            var dir = (target.position - topCenterPoint).normalized;
            var yAxis = -Physics.gravity.normalized;
            var offset = Vector3.ProjectOnPlane(dir, yAxis);
            var bottomCenterPoint = topCenterPoint - yAxis * mGradient;

            if (!IsConeContain(target.position))
                return false;

            if (Vector3.Distance(offset, Vector3.zero) < mError)
                offset = Vector3.ProjectOnPlane(Random.onUnitSphere, yAxis).normalized;

            var bottomPoint = bottomCenterPoint + offset * mGradient;

            var a = topCenterPoint;
            var b = bottomPoint - yAxis * mSpeedAreaBottomExtern;
            var c = target.transform.position;
            var finalSpeed = mMaxSpeed;

            if (Vector3.Distance(a, b) > Vector3.Distance(b, c))
            {
                var rate = (Vector3.Distance(b, c) / Vector3.Distance(a, b));
                finalSpeed = Mathf.Lerp(mMinSpeed, mMaxSpeed, rate);
            }

            var coneFixDir = (bottomPoint - topCenterPoint).normalized * finalSpeed * Time.fixedDeltaTime;

            target.position += coneFixDir;

            return true;
        }

        bool IsConeContain(Vector3 point)
        {
            var topCenter = mTransform.position + mBoxcastCacheBounds.center + Vector3.up * mGradient * 0.5f;
            var bottomCenter = mTransform.position + mBoxcastCacheBounds.center - Vector3.up * mGradient * 0.5f;
            var dir = (bottomCenter - topCenter).normalized;
            var a = Vector3.Project(topCenter, dir);
            var b = Vector3.Project(bottomCenter, dir);
            var c = Vector3.Project(point, dir);

            var rate = Mathf.Clamp01(Vector3.Distance(a, c) / Vector3.Distance(a, b));
            var currentRadius = Mathf.Lerp(0, mRadius, rate);

            return Vector3.Distance(bottomCenter + (c - b), point) < currentRadius;
        }
    }
}